#include "StdAfx.h"
#include "SimApp.h"
#include "MilimetersMetricMeasure.h"
#include "InchesMetricMeasure.h"
#include "MillingToolRotationsShutDown.h"
#include "BresehamLine3D.h"
#include "ProgramEnd.h"
#include "QuickMotion.h"
#include "SlowMotion.h"
#include "InitMachine.h"
#include "ToroidalMillingTool.h"
#include "SphericalMillingTool.h"
#include <typeinfo>
#include <fstream>
#include <ctype.h>
#include <ppl.h>
#include <exception>
#include <string>
#include <algorithm>
#include <assert.h>

using namespace Concurrency;
using namespace std;

#define GET_NEXT_LINE(file,line_output) {memset(&line_output,0,MAX_LINE_CHARS_COUNT);file.getline(line_output,MAX_LINE_CHARS_COUNT);}

int iCount =0;
SimApp::SimApp(UINT materialHeight,UINT materialWidth,UINT depth)
{
	x = y = z = (MY_DATATYPE)INIT_DATA;
	this->states = new vector<ConfigurationState*>();
	this->measure = nullptr;
	this->machine = new MillingMachine3C(materialHeight,materialWidth,depth);
	this->tool = nullptr;
	this->pathDrawElement = nullptr;
	UINT errSize = materialHeight+1;
	this->millingErrors = new ErrorInfo*[errSize];
	for(UINT i=0;i<errSize;++i)
	{
		this->millingErrors[i] = new ErrorInfo[materialWidth+1];
	}
}

SimApp::~SimApp(void)
{
	UINT errSize = this->machine->height+1;
	for(UINT i=0;i<errSize;++i)
		delete [] this->millingErrors[i];

	delete[] this->millingErrors;

	delete this->machine;
	if(this->measure!=NULL)
		delete this->measure;
	parallel_for_each(this->states->begin(),this->states->end(),[&](ConfigurationState * state){
		delete state;
	});
	delete this->states;
}

int SimApp::getFileLineNumbers(const char * fName)
{
	ifstream file(fName);
	int lineCount = 0;
	char line[MAX_LINE_CHARS_COUNT];
	while(file.getline(line,MAX_LINE_CHARS_COUNT))
		lineCount++;
	file.close();
	return lineCount;
}

int SimApp::parseFile(const char * fName,NotifyProgressCallback progressFunc,ErrorNotifyCallback errorNotifyFunc)
{
	this->states->clear();
	const UINT ERROR_LIMIT = 100;
	UINT errors = 0;
	ifstream file;
	int lineNo = 1;
	int totalLinesCount = -1;
	int instructionNumber;
	try
	{
		totalLinesCount = this->getFileLineNumbers(fName);
		file.open(fName);
		char line[MAX_LINE_CHARS_COUNT];
		GET_NEXT_LINE(file,line);
		getMetricMeasure(line);
		ZeroMemory(line,MAX_LINE_CHARS_COUNT);
		while(file.getline(line,MAX_LINE_CHARS_COUNT))
		{
			char n,type;
			n = line[0];
			int count = 1;
			while(!isalnum(line[count++]) && count<MAX_LINE_CHARS_COUNT){};
			if(count == MAX_LINE_CHARS_COUNT)
				throw exception("there could be no blank lines in machine file");
			instructionNumber = atoi(&line[count]);
			while(isdigit(line[count]))count++;
			type = line[count++];
			switch(type)
			{
			case 'G':
				parseInstruction(line,lineNo);
				break;
			case 'S':
				parseInitMsg(line,lineNo);
				break;
			case 'F':
				parseInitMsg(line,lineNo);
				break;
			case 'M':
				parseControlInstruction(line,lineNo);
				break;
			}
			ZeroMemory(line,MAX_LINE_CHARS_COUNT);
			if(progressFunc!=nullptr)
				progressFunc(lineNo*100/totalLinesCount);
			lineNo++;
		}
		file.close();
	}
	catch(exception & e)
	{
		file.close();
		ofstream o("errors.txt",ios_base::app);
		o<<e.what()<<"line no: "<<lineNo<<" ; instruction no: "<<instructionNumber<<" what: "<<e.what()<<endl;
		o.close();
		errors++;
		if(errors>=ERROR_LIMIT)
			return 0;
	}
	return 1;
}

MetricMeasure* SimApp::getMetricMeasure(const char * line)
{
	int len = strlen(line);
	if(strcmp(line,"%G71")==0)
		return new MilimetersMetricMeasure(1);
	else if(strcmp(line,"%G70")==0)
		return new InchesMetricMeasure(1);
	throw exception("file config does not select measure");
}

void SimApp::parseInitMsg(const char * line,int lineNo)
{
	char n,type;
	int instructionNumber;
	int speed;
	int count = 0;
	while(!isalnum(line[count++])){};
	n = line[count++];
	instructionNumber = atoi(&line[count]);

	while(isdigit(line[count]))count++;

	type = line[count++];

	if(type=='S')
	{
		speed = atoi(&line[count]);
		this->machine->toolRPM = speed;
		while(isdigit(line[count]))
		{
			count++;
		}
		if(isalpha(line[count]))
		{
			char *clockWiseRotation = "M03";
			if(strcmp(&line[count],clockWiseRotation)==0)
			{
				this->machine->toolRotatingClockwise = true;
			}
		}
	}
	else if(type=='F')
	{
		speed = atoi(&line[count]);
		this->machine->toolVelocity = speed;
	}
}

void SimApp::parseInstruction(const char * line,int lineNo)
{
	//coordinates initialization
	if(strcmp(line,"N1G40G90")==0)
	{
		this->states->push_back(new InitMachine(lineNo));
		return;
	}
	int count = 0;
	while(!isalnum(line[count++])){};
	char c = line[count-1];
	if(c!='N')
		throw exception("wrong path format");
	int instructionNumber;

	instructionNumber = atoi(&line[count]);
	while(isdigit(line[count]))++count;
	char commandType = line[count++];
	int commandId = atoi(&line[count]);

	while(isdigit(line[count]))++count;
	bool done=false;
	int i=0;
	while(!done && i<3)
	{
		char axis = line[count];
		switch(axis)
		{
		case 'X':
			{
				x = (MY_DATATYPE) atof(&line[count+1]);
				i++;
			}
			break;
		case 'Y':
			{
				y = (MY_DATATYPE) atof(&line[count+1]);
				i++;
			}
			break;
		case 'Z':
			{
				z = (MY_DATATYPE) atof(&line[count+1]);
				i++;
			}
			break;
		}
		if((line[count++]=='\n' && line[count-1]=='\r') ||count>=strlen(line))
			done = true;
	}
	if((this->pathCenter.x + x)>this->machine->width)
	{
		//sciezka wychodzi poza kontur!
		//throw exception("sciezka poza konturem");
	}else if(this->pathCenter.y +y > this->machine->height)
	{
		//sciezka wychodzi poza kontur!
		//throw exception("sciezka poza konturem");
	}
	if((this->pathCenter.y + y)<0)
		commandId = 0;
	MY_DATATYPE machineX,machineY,machineZ;
	transformPathPointToMachinePoint(x,y,z,&machineX,&machineY,&machineZ);
#if _DEBUG
	ofstream o("dbg.txt",ios_base::app);
	o<<instructionNumber<<" x: "<<x<<" y: "<<y<<" z: "<<z<<" machined x: "<<machineX<<" machined y: "<<machineY<<" machined z:"<<machineZ<<endl;
	o.flush();
	o.close();
#endif
	switch(commandId)
	{
	case 0:
		{
			ConfigurationState * cs	= this->states->back();
			//if(strcmp(typeid(*cs).name(),typeid(SlowMotion).name())==0)
			//	throw exception("cannot perform this action");
			this->states->push_back(new QuickMotion(instructionNumber,machineX,machineY,machineZ));
		}
		break;
	case 1:
		{
			this->states->push_back(new SlowMotion(instructionNumber,machineX,machineY,machineZ));
		}
		break;
	}
}

void SimApp::parseControlInstruction(const char * line,int lineNo)
{
	int count = 0;
	while(!isalnum(line[count++])){};
	char c = line[count++];
	//int strLength = strlen(line);
	int command = atoi(&line[count]);
	switch(command)
	{
	case 3:
		{
			this->machine->toolRotatingClockwise = true;
		}
		break;
	case 5:
		{
			this->states->push_back(new MillingToolRotationsShutDown(lineNo));
		}
		break;
	case 30:
		{
			this->states->push_back(new ProgramEnd(lineNo));
		}
		break;
	}
}

void SimApp::processStates(vector<ConfigurationState *> *& states,const Point3D & pathCenter,NotifyProgressCallback progressFuncNotify,ErrorNotifyCallback errorNotifyFunc)
{
	vector<ConfigurationState*> * processedResult = new vector<ConfigurationState*>();
	vector<ConfigurationState*>::iterator it;
	ConfigurationState * lastState = nullptr;
	int itCount = 1;
	for(it = states->begin();it!=states->end();++it)
	{
		processedResult->push_back((*it));
		if(lastState!=NULL)
		{
			//interpolacja
			SlowMotion * _lastState = dynamic_cast<SlowMotion*>(lastState);
			SlowMotion * _currentState = dynamic_cast<SlowMotion*>(*it);
			if(_lastState!=nullptr && _currentState!=nullptr)
			{
				BresehamLine3D * line = new BresehamLine3D(_lastState,_currentState);
				line->perform(this->machine,this->tool,processedResult,this->millingErrors);
				delete line;
			}
		}
		if(progressFuncNotify!=nullptr)
			progressFuncNotify(itCount*100/(int)states->size());
		++itCount;
		lastState = (*it);
	}
	states = processedResult;
}

UINT SimApp::GetMaterialSize()
{
	return this->machine->width*this->machine->height;
}

void SimApp::ProcessStates(NotifyProgressCallback progressNotifyFunc,ErrorNotifyCallback errorNotifyFunc)
{
	processStates(this->states,this->pathCenter,progressNotifyFunc,errorNotifyFunc);
	this->RewindAnitmation();
}

bool SimApp::PerformAnimationStep(NotifyProgressCallback progressFuncNotify,ErrorNotifyCallback errFuncNotify)
{
	//ofstream o("animStep.txt");
	//o<<animStep++<<endl;
	//o.close();
	if(this->states->size()==0)
		return true;
	this->machine->BindBuffer();

	do
	{
		(*this->currentState)->perform(this->machine,this->tool);
		this->currentState++;
	}while(this->currentState!=this->states->end() && !(*this->currentState)->isKeyState);

	if(this->currentState!=this->states->end())
		(*this->currentState)->perform(this->machine,this->tool);

	this->machine->UnbindBuffer();

	if(this->currentState == this->states->end())
	{
		this->currentState = this->states->begin();

		if(progressFuncNotify!=nullptr)
			progressFuncNotify(100);

		return true;
	}
	else
	{
		if(progressFuncNotify!=nullptr)
			progressFuncNotify((*this->currentState)->number*100/(int)this->states->size());
	}
	return false;
}

void SimApp::RewindAnitmation()
{
	assert(this->states!=NULL);
	this->currentState = this->states->begin();
	this->animStep = 0;
}

void SimApp::SetPathCenter(double x,double y,double z)
{
	this->pathCenter.x = (MY_DATATYPE) x;
	this->pathCenter.y = (MY_DATATYPE) y;
	this->pathCenter.z = (MY_DATATYPE) z;
}

void SimApp::transformPathPointToMachinePoint(double pathPointX,double pathPointY,double pathPointZ,MY_DATATYPE * machinePointX,MY_DATATYPE * machinePointY,MY_DATATYPE * machinePointZ)
{
	*machinePointX = (MY_DATATYPE) (this->pathCenter.x + pathPointX);
	*machinePointY = (MY_DATATYPE) (this->pathCenter.y + pathPointY);
	*machinePointZ = (MY_DATATYPE) (this->pathCenter.z + pathPointZ);
}

void SimApp::SetToolX(double val)
{
	this->x = (MY_DATATYPE) val;
	assert(this->tool!=nullptr);
	this->tool->posX = val;
}

void SimApp::SetToolY(double val)
{
	this->y = (MY_DATATYPE) val;
	assert(this->tool!=nullptr);
	this->tool->posY = val;
}

void SimApp::SetToolZ(double val)
{
	this->z = (MY_DATATYPE) val;
	assert(this->tool!=nullptr);
	this->tool->posZ = val;
}

double SimApp::GetToolX()
{
	assert(this->tool!=nullptr);
	return this->tool->posX;
}

double SimApp::GetToolY()
{
	assert(this->tool!=nullptr);
	return this->tool->posY;
}

double SimApp::GetToolZ()
{
	assert(this->tool!=nullptr);
	return this->tool->posZ;
}

void SimApp::CreateToroidalTool(double length,int diameter)
{
	if(this->tool!=nullptr)
	{
		delete this->tool;
	}
	this->tool = new ToroidalMillingTool(diameter,length);
}

void SimApp::CreateBallTool(double length,int diameter)
{
	if(this->tool!=nullptr)
	{
		delete this->tool;
	}
	this->tool = new SphericalMillingTool(length,diameter);
}

PathsDrawElement* SimApp::GetToolPathDrawElement()
{
	if(this->states==nullptr && this->pathDrawElement==nullptr)
		throw exception("machine states could not be retrieved. Please parse G-Code machine file first");
	if(this->pathDrawElement==nullptr)
	{
		this->pathDrawElement = new PathsDrawElement(this->states);
	}
	return this->pathDrawElement;
}

UINT SimApp::getCurrentInstructionNumber()
{
	if(this->currentState!=this->states->end())
		return (*this->currentState)->number;
}

void SimApp::CreateMaterial(UINT materialWidth,UINT materialHeight,UINT initDepth)
{
	if(this->machine!=nullptr)
		delete this->machine;
	this->machine = new MillingMachine3C(materialWidth,materialHeight,initDepth);
}